#include "Ano_Imu_Data.h"
#include "Ano_Imu_Calibration.h"
#include "Ano_Imu_Task.h"
#include "Drv_Bmi088.h"
#include "Drv_spl06.h"
#include "Ano_Parameter.h"

/*IMU循环任务1毫秒*/
//每毫秒执行一次的核心 IMU 数据处理任务
void ImuServices_1ms_c()
{ 
	/*读取陀螺仪加速度计数据*/
	DrvBmi088AccelerationRead();//用于从 加速度计 寄存器中读取原始数据。
	
	//Drv_Icm20602_Read();
	DrvBmi088ReadServices();//用于从 陀螺仪计 寄存器中读取原始数据。
	
	//传入数据
	//将原始数据传入 IMU 数据结构体
	//保存到全局变量 st_imuData 中。
	ImuDataGet((s16 *)st_bmi_data.un_ins.st_data.s16_gyr,
	           (s16 *)st_bmi_data.un_ins.st_data.s16_acc);
	
	
	//数据计算
	//	ImuDataCalcu() 是核心函数，负责：
	//校准（减去偏移）
	//单位换算（原始值 → deg/s 或 cm/s²）
	//坐标变换（IEM矩阵乘法）
	//弧度制转换（供姿态解算使用）
	ImuDataCalcu(Ano_Parame.set.acc_calibrated,
	             (float *)Ano_Parame.set.gyr_zero_offset,
							 (float *)Ano_Parame.set.acc_zero_offset,
							 (float (*)[])Ano_Parame.set.IEM);
	
	//
	if(flag.unlock_sta==0)//表示飞行器尚未解锁（未起飞），可以安全进行校准操作
	{
		
		//静止检测
		//用于判断加速度和角速度是否稳定，防止在运动状态下误触发校准
		AccGyrStableCheck_Services(0.001f,st_imuData.f_acc_cmpss,st_imuData.f_gyr_dps);
		//
		if(st_imu_cali.gyr_cali_on)
		{
			//
			if(GetGyrAvValue(st_imu_cali.gyr_stable,st_imuData.f_gyrRaw,(float *)Ano_Parame.set.gyr_zero_offset))
			{
				if(st_imu_cali.gyr_cali_on==2)
				{
					st_imu_cali.gyr_cali_on = 0;
				}
				else if(st_imu_cali.gyr_cali_on==1)
				{
					st_imu_cali.gyr_cali_on = 0;
					//存储参数
					data_save();
				}
			}
		}
	}
}

/*IMU循环任务20毫秒*/
//每 20 毫秒执行一次的辅助任务（如温度获取）
void ImuServices_20ms_c()
{
	//获取IMU温度
	ImuTemperatureGet(spl_data.temperature);
	//
}
